#! /usr/bin/env python3
#coding=utf-8
from sensor_msgs.msg import LaserScan
from mcq_msgs.msg import Carry

import rospy
import rospkg
import cv2
import math
import matplotlib.pyplot as plt
import matplotlib.colors as mcolors
import matplotlib.patches as patches
from matplotlib.animation import FuncAnimation, FFMpegWriter
import matplotlib.animation as animation
import numpy as np
import sys
import os
import select

def RAD2DEG(x):
    return x * 180.0/math.pi


angles = []
distances = []  
cluster = []
centerX = []
centerY = []  
radius = []
def DB_callback(msg):
    global angles,distances,cluster,centerX,centerY,radius
    angles = list(msg.angles)
    distances = list(msg.DB_data)
    cluster = list(msg.cluster)
    centerX = list(msg.centerX)
    centerY = list(msg.centerY)
    radius = list(msg.radius)
    # for i in range(len(cluster)):
    #     print(f"类别：{cluster[i]},x:{angles[i]},y:{distances[i]}"

if __name__ == '__main__':
    rospy.init_node("take_DB_node",anonymous=True)
    sub_DBdata = rospy.Subscriber("/mcq_DB_topic",Carry,DB_callback,queue_size=1000)

    rate = rospy.Rate(10)
    maximum_distance = 200
    # 直角坐标下
    # fig, ax = plt.subplots(figsize=(10, 10))
    # while not rospy.is_shutdown():
    # def update(frame):
    #     if angles and distances and cluster:
    #         ax.clear()
    #         # 使用颜色映射
    #         norm = mcolors.Normalize(vmin=min(cluster), vmax=max(cluster))
    #         cmap = plt.cm.get_cmap('viridis')
    #         colors = [cmap(norm(category)) if category != 0 else 'red' for category in cluster]
    #         # 绘制直角坐标系下的散点图
    #         ax.scatter(angles, distances, c=colors)
    #         # 设置坐标轴范围，以 (0, 0) 为中心
    #         ax.set_xlim(-maximum_distance, maximum_distance)
    #         ax.set_ylim(-maximum_distance, maximum_distance)
    #         #  # 设置 x 和 y 轴的刻度间距
    #         # x_ticks = np.arange(-maximum_distance, maximum_distance + 1, step=10)
    #         # y_ticks = np.arange(-maximum_distance, maximum_distance + 1, step=10)
    #         # ax.set_xticks(x_ticks)
    #         # ax.set_yticks(y_ticks)
    #         # 绘制圆
    #         # for i in range(max(cluster)+1):
    #         #     circle_center = (centerX[i], centerY[i])
    #         #     circle_radius = radius[i]
    #         #     circle_color = circle_color = cmap(norm(cluster[i])) if cluster[i] != 0 else 'red'
    #         #     circle = patches.Circle(circle_center, circle_radius, fill=False, edgecolor=circle_color, linestyle='--')
    #         #     ax.add_patch(circle)
    #             # 添加文本标注
    #         for category in set(cluster):
    #             # 获取该类别的索引
    #             indices = [i for i, value in enumerate(cluster) if value == category]
    #             if(category == 0):continue
    #             # 计算该类别的平均位置
    #             avg_x = np.mean([angles[idx] for idx in indices])
    #             avg_y = np.mean([distances[idx] for idx in indices])
    #             # 添加标签
    #             ax.text(avg_x, avg_y, f'Class {category}', fontsize=8, color='black', ha='center', va='center')
    #         # 添加网格线
    #         ax.grid(True)
    #         ax.set_xlabel('X轴')
    #         ax.set_ylabel('Y轴')
    #         ax.set_aspect('equal', adjustable='datalim')
    #         # return ax
    #         # ax.legend() 
    #         plt.pause(0.1)
    #         # if sys.stdin in select.select([sys.stdin], [], [], 0)[0]:
    #         #     input_char = sys.stdin.read(1)
    #         #     break
    #         # rate.sleep()
    # ani = animation.FuncAnimation(fig, update, frames=range(10000), interval=20)
    # ani.save('/home/mcq/ros1_lidar/src/rplidar_ros/scripts/video/scatter_animation6.mp4', writer='ffmpeg', fps=10)
    # sub_DBdata.unregister()  # 取消订阅话题
    # plt.close(fig)   


    # 极坐标下
    plt.ion()
    fig = plt.figure(figsize=(10,10))
    ax = fig.add_subplot(111,polar=True)
    rate = rospy.Rate(10)
    maximum_distance = 100
    while not rospy.is_shutdown():
        if angles and distances:
            ax.clear()
            ax.set_theta_offset(-np.pi / 2)
            # 将角度转换为弧度
            angles_radians = np.deg2rad(angles)
            # 使用颜色映射
            norm = mcolors.Normalize(vmin=min(cluster), vmax=max(cluster))
            cmap = plt.cm.get_cmap('viridis')  # 选择颜色映射，可以根据需要选择其他颜色映射
            colors = [cmap(norm(category)) if category != 0 else 'red' for category in cluster]
            # 绘制极坐标图
            ax.scatter(angles_radians, distances,c=colors)
            ax.set_rlabel_position(90)
            ax.legend()
            ax.set_ylim([0, maximum_distance])
            plt.pause(0.01)
    rate.sleep()